#include<iostream>

using namespace std;

// 参数
const int FX = 310, FY = 250;
const int CX = 160, CY = 120;
const int IMG_HGT = 640, IMG_WID = 480;
const int IMG_SZ = IMG_HGT*IMG_WID;

class dep_to_pcl_c
{
public:
	float *tab_u; // 存储像素x轴坐标
	float *tab_v; // 存储像素y轴坐标
	
	// 深度图转化为点云
	dep_to_pcl_c()
	{
		tab_u = new float[IMG_SZ];
		tab_v = new float[IMG_SZ];
		
		for(long v=0; v < IMG_HGT; v++) {
			for(long u=0; u<IMG_WID; u++) {
				long n = u + v*IMG_WID;
				tab_u[n] = ((float)(u-CX))/((float)FX);
				tab_v[n] = ((float)(v-CY))/((float)FY);
			}
		}
	}
	
	// img_dep数据单元[d1, d2, ..., ]
	// img_pcl数据单元[x, y, z, x, y, z, ...]
	void operator()(float *img_dep, float *img_pcl)
	{
		float *p = img_pcl;
		float *q = img_dep;
		float *qe = img_dep+IMG_SZ;
		long n = 0;
		while(q < qe)
		{
			float z = *q;	// 获取深度值
			*p = z*tab_u[n]; p++;	// 计算X
			*p = z*tab_v[n]; p++;	// 计算Y
			*p = z;
			q++; n++;
		}
	}
	
	~dep_to_pcl_c()
	{
		delete []tab_u;
		delete []tab_v;
	}
};